function C = heatmap(x,data,intensity)
% Create a heatmap plot
% HEATMAP(X,DATA,INTENSITY) returns the image matrix C of the heatmap
% generated
% 
% X is the x-axis vector.
% DATA is an arbitrarily long list of trajectories. Length must match that of X.
% INTENSITY is an optional parameter that allows for data to be 'brightened' more quickly, but will cause the colors to saturate.
%
% The returned matrix C is a square matrix with elements between 0 and 1.0
% that correspond to the intensity or 'heat' of the heatmap at that point.
if nargin==2
    intensity = 1.0;
end

res = 500; %resolution of image produced.

xmax = max(x);
xmin = min(x);
points = length(data(:,1)); %number of data sets
C = zeros(res,res); %image matrix

yi = zeros(points,res);
h = (xmax-xmin)/res; %horizontal spacing
xi = xmin+h:h:xmax; %create x-axis that matches image 'pixels'

for i=1:points
    yi(i,:) = interp1(x,data(i,:),xi,'linear'); %get y data that matche image 'pixels'
end

ymin = min(min(yi));
ymax = max(max(yi));
v = (ymax-ymin)/res; % vertical spacing
for i=1:points
    for j=1:res
        index = 1+floor((yi(i,j)-ymin)/v); %find vertical location of data point
        if index >= res
            index = res-1;
        end
        C(res-index,j)=C(res-index,j)+(1.0/points); %image flips vertically, so have to do it backwards
        if C(res-index,j) > 1.0 %saturation of element. Should only occur when intensity > 1
            C(res-index,j) = 1.0;
        end
    end
end
cmax = max(max(C));
C = C*intensity/cmax; %scale colors to have max color match max element
image(C)
ticks = 10;
th = res/ticks;
xlabs = xmin:h*th:xmax;
set(gca,'XTick',0:res/ticks:res);
set(gca,'XTickLabel',xlabs);

ylabs = ymin:v*th:ymax;
set(gca,'YTick',0:res/ticks:res);
set(gca,'YTickLabel',fliplr(ylabs));
